import heapq


class Node:
    def __init__(self, x, y, cost, heuristic):
        self.x = x
        self.y = y
        self.cost = cost
        self.heuristic = heuristic

    def __lt__(self, other):
        return (self.cost + self.heuristic) < (other.cost + other.heuristic)


def astar(start, goal, grid):
    rows = len(grid)
    cols = len(grid[0])
    directions = [(1, 0), (-1, 0), (0, 1), (0, -1)]
    visited = set()
    start_node = Node(start[0], start[1], 0,
                      abs(start[0] - goal[0]) + abs(start[1] - goal[1]))
    pq = []
    heapq.heappush(pq, start_node)

    while pq:
        current = heapq.heappop(pq)
        if (current.x, current.y) == goal:
            return current.cost
        if (current.x, current.y) in visited:
            continue
        visited.add((current.x, current.y))
        for direction in directions:
            next_x = current.x + direction[0]
            next_y = current.y + direction[1]
            if 0 <= next_x < rows and 0 <= next_y < cols and grid[next_x][next_y] == 0:
                next_cost = current.cost + 1
                next_heuristic = abs(next_x - goal[0]) + abs(next_y - goal[1])
                next_node = Node(next_x, next_y, next_cost, next_heuristic)
                heapq.heappush(pq, next_node)
    return -1


# 示例网格，0 表示可通行，1 表示障碍物
grid = [
    [0, 0, 0, 0],
    [0, 1, 1, 0],
    [0, 0, 0, 0],
    [0, 1, 1, 0]
]
start = (0, 0)
goal = (3, 3)
result = astar(start, goal, grid)
if result!= -1:
    print(f"The shortest path cost is: {result}")
else:
    print("There is no valid path from start to goal.")

